import sys
sys.path.append('/home/pi/MasterPi')
import time
import signal
import numpy as np
import pandas as pd
import HiwonderSDK.Sonar as Sonar
import HiwonderSDK.Board as Board
import HiwonderSDK.mecanum as mecanum
import math

chassis = mecanum.MecanumChassis()
HWSONAR = Sonar.Sonar()

# 状态定义
# 0: 前进
# 1: 检测到障碍右平移
# 2: 左旋转90度
# 3: 继续向右平移
# 4: 直行
# 5: 直行一段时间后向右旋转90度

# 参数配置
Threshold = 25  # 避障阈值
speed = 40  # 前进速度
direction = 0
__isRunning = False  # 运行状态标志
__until = 0  # 时间控制变量


# 新增全局变量
state = 0  # 初始状态为前进
t1 = 0  # 第一次遇到障碍的时间
t2 = 0  # 脱离障碍的时间
rotation_start_time = 0  # 旋转开始时间
distance_data = []  # 距离数据缓存
forward = True  # 动作执行标志
stopMotor = True  # 停止电机标志
old_speed = 0  # 前一次速度


def get_state():
    global state, speed, direction
    return state, speed, direction


# 初始位置
def initmove():
    chassis.set_velocity(0, 0, 0)  # 停止底盘运动


# 变量重置
def reset():
    global __isRunning, Threshold, speed, stopMotor, forward, old_speed, state
    global distance_data, t1, t2, rotation_start_time

    speed = 40
    old_speed = 0
    Threshold = 15.0
    forward = True
    stopMotor = True
    __isRunning = False
    state = 0
    distance_data = []
    t1 = 0
    t2 = 0
    # rotation_start_time = 0


# 初始化调用
def init():
    print("Avoidance Init")
    initmove()
    reset()


# 开始玩法调用
def start():
    global __isRunning, forward, stopMotor

    __isRunning = True
    forward = True
    stopMotor = True
    print("Avoidance Start")


# 停止玩法调用
def stop(signum, frame):
    global __isRunning

    __isRunning = False
    chassis.set_velocity(0, 0, 0)
    print("Avoidance Stop")


# 退出玩法调用
def exit():
    global __isRunning
    __isRunning = False
    chassis.set_velocity(0, 0, 0)
    HWSONAR.setPixelColor(0, Board.PixelColor(0, 0, 0))
    HWSONAR.setPixelColor(1, Board.PixelColor(0, 0, 0))
    print("Avoidance Exit")


def detect_obstacle():
    global speed, __until, __isRunning, Threshold, direction
    global distance_data, stopMotor, forward, old_speed
    global state, t1, t2, rotation_start_time

    # 读取超声波距离并转换为厘米
    dist = HWSONAR.getDistance() / 10.0

    # 距离数据滤波处理
    distance_data.append(dist)
    data = pd.DataFrame(distance_data)
    data_ = data.copy()
    u = data_.mean()  # 计算均值
    std = data_.std()  # 计算标准差

    data_c = data[np.abs(data - u) <= std]  # 过滤异常值
    distance = data_c.mean()[0]

    if len(distance_data) == 5:  # 维护滑动窗口，保留最近5个数据点
        distance_data.remove(distance_data[0])

    if distance <= Threshold:
        obstacle = 1
    else:
        obstacle = 0

    return obstacle


def run():
    global speed, __until, __isRunning, Threshold, direction
    global distance_data, stopMotor, forward, old_speed
    global state, t1, t2, rotation_start_time

    if not __isRunning:
        if stopMotor:
            stopMotor = False
            chassis.set_velocity(0, 0, 0)  # 关闭所有电机
            direction = 0
        forward = True
        time.sleep(0.03)
        return

    # 新避障状态机current_time = time.time()
    # current_time = time.time()

    # 状态0: 前进状态
    if state == 0:
        # 设置前进速度 (方向0°为正前方)
        if speed != old_speed:
            old_speed = speed
            chassis.set_velocity(speed, 90, 0)
            direction = 90

        # 检测到障碍
        if detect_obstacle():
            chassis.set_velocity(0, 0, 0)  # 停止前进
            direction = 0
            t1 = time.time()  # 记录遇到障碍时间
            state = 1  # 进入避障状态
            forward = True
            print("检测到障碍，进入1")

    # 状态1: 检测到障碍右平移
    elif state == 1:
        # 第一次进入此状态，启动右移
        if forward:
            forward = False
            # 向右平移 (方向90°)
            chassis.set_velocity(speed, 0, 0)
            direction = 0
            __until = time.time() + 0.1  # 设置右移1秒
            print("右移0.1s")

        # 右移时间结束
        if time.time() >= __until:
            # 再次测距
            if detect_obstacle() == 1:
                # 仍然有障碍，继续右移1秒
                __until = time.time() + 0.1
                print("仍有障碍")
                print(detect_obstacle())
            if detect_obstacle() == 0:
                # 脱离障碍，记录时间并进入下一状态
                chassis.set_velocity(speed, 0, 0)
                direction = 0
                time.sleep(0.5)
                t2 = time.time()
                state = 2
                forward = True  # 重置标志
                print("脱离障碍")

    # 状态2: 左旋转90度
    elif state == 2:
        if forward:
            forward = False
            # 停止平移，开始向左旋转90度
            chassis.set_velocity(0, 0, 0)
            chassis.set_velocity(0, 90, -0.5)  # 左旋转(正值为逆时针)
            direction = 90
            # rotation_start_time = current_time
            __until = time.time() + math.pi/6  # 旋转90度需要π秒 (0.5 rad/s)
            print("2.正在左旋")

        elif time.time() >= __until:
            chassis.set_velocity(speed, 0, 0)
            direction = 0
            time.sleep(0.5)
            state = 3
            forward = True
            print("左旋结束，进入3")

    # 状态5: 继续向右平移
    elif state == 3:
        if forward:
            forward = False
            # 继续向右平移 (方向90°)
            chassis.set_velocity(speed, 0, 0)
            direction = 0
            __until = time.time() + 0.1  # 设置右移0.1秒
            print("3.右移0.1秒")

        # 右移时间结束
        if time.time() >= __until:
            # 再次测距
            if detect_obstacle() == 1:
                # 仍然有障碍，继续右移1秒
                __until = time.time() + 0.1
                print("3.仍有障碍，继续右移")

            if detect_obstacle() == 0:
                # 脱离障碍，进入下一状态
                chassis.set_velocity(speed, 0, 0)
                direction = 0
                time.sleep(0.5)
                state = 4
                forward = True
                print("3.障碍已避开，进入状态4")

    # 状态4: 直行
    elif state == 4:
        if forward:
            forward = False
            # 计算需要前进的时间
            forward_time = t2 - t1
            if forward_time > 0:
                # 向前进方向移动补偿时间
                chassis.set_velocity(speed, 90, 0)
                direction = 90
                __until = time.time() + forward_time
                print("4.直行")

            else:
                # 无效时间，直接进入旋转
                state = 5
                forward = True

        elif time.time() >= __until:
            state = 5
            forward = True
            print("直行完成，进入5")

    # 状态5: 向右旋转90度
    elif state == 5:
        if forward:
            forward = False
            # 开始向右旋转90度
            chassis.set_velocity(0, 0, 0)
            chassis.set_velocity(0, 90, 0.5)  # 右旋转(负值为顺时针)
            direction = 90
            # rotation_start_time = current_time
            __until = time.time() + math.pi/5.2  # 旋转90度需要π秒 (0.5 rad/s)
            print("5.右旋转90度")

        elif time.time() >= __until:
            chassis.set_velocity(0, 0, 0)  # 停止旋转
            direction = 0
            state = 0  # 返回前进状态
            forward = True  # 重置标志
            print("右旋转完成，返回状态0: 前进")


def avoidance_run():
    init()
    start()
    signal.signal(signal.SIGINT, stop)
    try:
        print('AVOIDANCE ENTERED')
        while __isRunning:
            run()
            time.sleep(0.05)  # 控制循环频率
    except KeyboardInterrupt:
        pass
    finally:
        exit()


if __name__ == '__main__':
    init()
    start()
    signal.signal(signal.SIGINT, stop)
    try:
        while __isRunning:
            run()
            time.sleep(0.05)  # 控制循环频率
    except KeyboardInterrupt:
        pass
    finally:
        exit()


